
#include "ubt_mqtt/config.h"
#include "ament_index_cpp/get_package_share_directory.hpp"

namespace ubt {


ConfigDataInfo& ConfigDataInfo::Instance()
{
    static ConfigDataInfo instance;
    return instance;
}

ConfigDataInfo::ConfigDataInfo()
{
}

ConfigDataInfo::~ConfigDataInfo()
{
}

bool ConfigDataInfo::Initialize()
{
    node->declare_parameter<std::string>("agv_name",g_robot_name);
    _robot_name = node->get_parameter("agv_name").as_string();
    node->declare_parameter<std::string>("robot_sn",g_robot_sn);
    _robot_sn = node->get_parameter("robot_sn").as_string();
    node->declare_parameter<std::string>("agv_loc_algs",g_robot_type);
    _robot_type = node->get_parameter("agv_loc_algs").as_string();

    node->declare_parameter<std::string>("/system/map_yaml_path","/home/ubt/ubt_agv_local_paramter/maps/current_map.yaml");
    _map_origin_path = node->get_parameter("/system/map_yaml_path").as_string();
    node->declare_parameter<std::string>("/system/map_data_dir","/home/ubt/ubt_agv_local_paramter/maps/");
    _map_image_path = node->get_parameter("/system/map_data_dir").as_string();
    node->declare_parameter<std::string>("/system/map_localpoint","/home/ubt/ubt_agv_local_paramter/maps/localpoint.yaml");
    _localpoint_file_path = node->get_parameter("/system/map_localpoint").as_string();
    node->declare_parameter<std::string>("/system/server_paramter","/home/ubt/ubt_agv_local_paramter/maps/server_paramter.yaml");
    _server_paramer_path = node->get_parameter("/system/server_paramter").as_string();


    node->declare_parameter<std::string>("/system/cfg","/home/ubt/ubt_agv_install/config");
    std::string _server_cfg_path = node->get_parameter("/system/cfg").as_string();

    _map_image_path   = _map_image_path + "test2000_map.pgm";
    _task_file_path   = _server_cfg_path + g_package_name+"/task.yaml";
    _config_file_path = _server_cfg_path + g_package_name+ "/uiconfig.yaml";
    _fixedpath_file_path = _server_cfg_path + "/ubt_trajectory_track/path_info.yaml";

    RCLCPP_INFO_STREAM(node->get_logger(),"_server_paramer_path: " << _server_paramer_path);

    return true;
}

} // namespace ubt


